﻿/*****************************************************************//**
 * \file   SurgicalHapticThread.cpp
 * \brief  处理haptic device 线程类
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#include "./include/SurgicalHapticThread.h"
#include <unistd.h>

 /**
  * \brief   SurgicalHapticThread类构造函数.
  *
  */
SurgicalHapticThread::SurgicalHapticThread()
{

}

/**
 * \brief   SurgicalHapticThread类析构函数.
 *
 */
SurgicalHapticThread::~SurgicalHapticThread()
{

}

/**
 * \brief   槽函数.
 *
 */
void SurgicalHapticThread::virtualDeviceOpen()
{
	bVirtualFixturesOpen = true;
}

/**
 * \brief   槽函数.
 *
 */
void SurgicalHapticThread::virtualDeviceClose()
{
	bVirtualFixturesOpen = false;
}

/**
 * \brief   触力处理线程.
 *
 */
void SurgicalHapticThread::run_code()
{
    //LARGE_INTEGER liFrequency, BeginTime, EndTime;
    struct timespec BeginTime, EndTime;

    //QueryPerformanceFrequency((LARGE_INTEGER*)&liFrequency);

	//SetThreadAffinityMask(GetCurrentThread(), 4);

	while (true)
	{
		//实际运行时需要去掉，比较耗时。
		//qDebug() << "SurgicalHapticThread runing";
//		WaitForSingleObject(hHapticSemaphore, INFINITE);
//		QueryPerformanceCounter((LARGE_INTEGER*)&BeginTime);
        sem_wait(&hHapticSemaphore);

        // 获取开始时间
        clock_gettime(CLOCK_MONOTONIC, &BeginTime);

		iSurgical++;

		if (1 /*bVirtualFixturesOpen*/)
		{
			v3dCompensateZ << 0, 0, 0.008;
			/***************************** 对切除路径产生吸引力的引导型虚拟夹具 *****************************/
			polypResectionPath.getMinDistancePoint(sRobotRunData[0].sRobotDy.v3dToolPosture + v3dCompensateZ);
			polypResectionPath.computeScaleFactor();
			v3dXdeta = polypResectionPath.v3dPointMindistance - (sRobotRunData[0].sRobotDy.v3dToolPosture + v3dCompensateZ);  //目标切除路径最近点到手术器械末端的位移

#if 0
			for (int i = 0; i < 3; i++) {   //将器械末端到切除路径最近点的位移的写入到txt文件中，方便计算最短距离
				g_distance_reord << xDeta(i) << "\t";
			}
			g_distance_reord << std::endl;
#endif
			polypPathStart = std::chrono::system_clock::now(); //计时器的起始时刻
			polypPathTimer = polypPathStart - polypPathEnd;//polyppath_timer即为从这一时刻位移到另一时刻位移所花费的时间

			v3dXDetaNow = v3dXdeta;//计时器的起始时刻对应的位移

			//引导虚拟夹具的引导力
			if (bFirstTimeCompute == false) {   //只有 f =  K * x 
				v3dForceGVF = polypResectionPath.dSpring_k * v3dXdeta;
			}
			else {   //都有 f =  K * x - D * V
				v3dForceGVF = polypResectionPath.dSpring_k * v3dXdeta - polypResectionPath.dDamping_d * (v3dXDetaNow - v3dXDetaLast) / (polypPathTimer.count());
			}

			v3dForceSpring = polypResectionPath.dSpring_k * v3dXdeta;
			v3dForceDamp = -polypResectionPath.dDamping_d * (v3dXDetaNow - v3dXDetaLast) / (polypPathTimer.count());

			v3dXDetaLast = v3dXdeta;
			polypPathEnd = std::chrono::system_clock::now();  //计时器的结束时刻对应的位移


			/***************************** 对非切除区域产生排斥力的禁止区域型虚拟夹具 *****************************/
			polypCloud.getMinDistancePoint(sRobotRunData[0].sRobotDy.v3dToolPosture + v3dCompensateZ);
			polypCloud.computeScaleFactor();

			v3dXdetaCloud = polypCloud.v3dPointCloudMindistance - (sRobotRunData[0].sRobotDy.v3dToolPosture + v3dCompensateZ);
#if 0
			for (int i = 0; i < 3; i++) {   //将器械末端到切除路径最近点的位移的写入到txt文件中，方便计算最短距离
				fr_distance_reord << xDetaCloud(i) << "\t";
			}fr_distance_reord << std::endl;
#endif
			v3dForceFRVF = -polypCloud.B_FR_X * 10 * v3dXdetaCloud / polypCloud.l_MAX;


			/***************************** GVF生成的吸引力 + FRVF生成的排斥力 *****************************/


			//将在机械臂基坐标系下的虚拟夹具的力向量，转换到主手坐标系下
			v3dForceGVF = m3dRotationSimga2robot.inverse() * v3dForceGVF;
			v3dForceFRVF = m3dRotationSimga2robot.inverse() * v3dForceFRVF;

			//如果器械末端不在需要切除区域以内，就会有FRVF生成的排斥力，在一定区域内就会只有吸引力，而没有排斥力
			/*toolPx = g_RobotControl[0].robotDYparameter.robotDy.tool_posture(0);
			toolPy = g_RobotControl[0].robotDYparameter.robotDy.tool_posture(1);
			if (sqrt(pow(tool_px - polyp_CoordinateOrigin(0), 2) + pow(tool_py - polyp_CoordinateOrigin(1), 2)) <= 0.0259) {
				force_FRVF.setZero();
			}*/

			if (fabs(v3dForceFRVF(0)) >= dFRVFforceMax) {
				if (v3dForceFRVF(0) >= dFRVFforceMax) {
					v3dForceFRVF(0) = dFRVFforceMax;
				}
				else {
					v3dForceFRVF(0) = -dFRVFforceMax;
				}
			}
			if (fabs(v3dForceFRVF(1)) >= dFRVFforceMax) {
				if (v3dForceFRVF(1) >= 0) {
					v3dForceFRVF(1) = dFRVFforceMax;
				}
				else {
					v3dForceFRVF(1) = -dFRVFforceMax;
				}
			}
			if (fabs(v3dForceFRVF(2)) >= dFRVFforceMax) {
				if (v3dForceFRVF(2) >= 0) {
					v3dForceFRVF(2) = dFRVFforceMax;
				}
				else {
					v3dForceFRVF(2) = -dFRVFforceMax;
				}
			}

			if (fabs(v3dForceGVF(0)) >= dGVFforceMax) {
				if (v3dForceGVF(0) >= 0) {
					v3dForceGVF(0) = dGVFforceMax;
				}
				else {
					v3dForceGVF(0) = -dGVFforceMax;
				}
			}
			if (fabs(v3dForceGVF(1)) >= dGVFforceMax) {
				if (v3dForceGVF(1) >= 0) {
					v3dForceGVF(1) = dGVFforceMax;
				}
				else {
					v3dForceGVF(1) = -dGVFforceMax;
				}
			}
			if (fabs(v3dForceGVF(2)) >= dGVFforceMax) {
				if (v3dForceGVF(2) >= 0) {
					v3dForceGVF(2) = dGVFforceMax;
				}
				else {
					v3dForceGVF(2) = -dGVFforceMax;
				}
			}

			//forceGVF << 0, 0, 0;   //单独测试排斥力
			//forceFRVF << 0, 0, 0;   //单独测试吸引力

			dGuidingForceScale = (v3dXdetaCloud.norm()) / (v3dXdeta.norm());
			if (dGuidingForceScale > 1) {
				dGuidingForceScale = 1;
			}
			dGuidingForceScale = pow(dGuidingForceScale, 3);
			v3dForceVF = dGuidingForceScale * v3dForceGVF + (1 - dGuidingForceScale) * v3dForceFRVF;
			//forceVF = forceFRVF;

#if 0
			for (int i = 0; i < 3; i++) {   //将引导力力写入到txt文件中
				GVF_force_record << forceGVF(i) << "\t";
			}GVF_force_record << std::endl;


			for (int i = 0; i < 3; i++) {   //将排斥力产生的力写入到txt文件中
				FRVF_force_record << forceFRVF(i) << "\t";
			}FRVF_force_record << std::endl;
#endif


			v3dForceVF << 0, 0, 0;    //是否是自由操控
			//surgicalRobotCom->g_HapticControl[0].force = force_VF;
			surgicalRobotCom->HDControl[0].force = v3dForceVF;



			//SRobotHapticDeviceData sHapticRunData;
			if (surgicalRobotCom->HDControl[0].bHapticInitDone)
			{
				surgicalRobotCom->HDControl[0].GetHapticPosture(sHapticRunData[0]);
				//g_HapticControl[0].force << 0, 0, 0;
				surgicalRobotCom->HDControl[0].SetForceAndTorqueAndGripperForce(surgicalRobotCom->HDControl[0].force);//丢给simga.7

				surgicalRobotCom->HDControl[0].SetGravityCompensation();
			}
#if 0
			for (int i = 0; i < 3; i++) {
				toolend_record << sRobotRunData[0].robotDy.tool_posture(i) + compensate_z(i) << "\t";
			}toolend_record << std::endl;
#endif
			experiment01End = std::chrono::system_clock::now(); //计时器的结束时刻
			experiment01Spend = experiment01End - experiment01Start;
#if 0
			time_experimrnt << experiment01Spend.count() << "\t" << guidingforce_scale << std::endl; //将花费的时间记录下来
#endif
		}
		else {

            sleep(40);
		}
        //QueryPerformanceCounter((LARGE_INTEGER*)&EndTime);
        // 获取结束时间
        clock_gettime(CLOCK_MONOTONIC, &EndTime);

        // 计算时间差
        double elapsedTime = (EndTime.tv_sec - BeginTime.tv_sec) +
                                     (EndTime.tv_nsec - BeginTime.tv_nsec) / 1e9;
	}

}
